/*
Work in this file belongs to ROUTE PLANNING group. 
Other groups are not allowed to make changes on this file. 
During development, you can use your own test code that
includes your own setup() and loop() functions but they 
should not be included when you are submitting your changes. 
*/

// The function to determine nextStation according to the currentStation the car has been

int getNextStation(int currentStation) {
   int temp;
   int i = 0;
   do{
     temp = route[i];
     i++;
   }while(temp!=currentStation);
   
   return route[i];
  
}
/*
  Plan Route is the main fucntion determines for every road from current to destination:
  Includes :
    1. Handler for exit from Station.
    2. while loop constructs the directionArray station by srarion.
    3. for loop determines the crossroads between stations from the local table called directionTable
    4. if controls whether the car arrived the destination or not to be enter station again or pass the station.

*/
void rpPlanRoute(){ 

byte directionTable [4][3] = {

	{1,2,STRAIGHT},
	{3,1,TURNLEFT},
	{3,4,STRAIGHT},
	{5,2,STRAIGHT}
};

int index = 0;
int i;  

//    1. Handler for exit from Station.
directionArray[index] = TURNLEFT;
index++;
rpNextStation = getNextStation(rpCurrentStation);


// ve yol bilgileri optimizasyondan alınacak.
//    2. while loop constructs the directionArray station by srarion.

while(rpCurrentStation != rpDestinationStation) {
//    3. for loop determines the crossroads between stations from the local table called directionTable

    for(i=0;i<4;i++) { 
        if(rpCurrentStation == directionTable[i][0] && rpNextStation == directionTable[i][1] ) {
	    directionArray[index] = directionTable[i][2];
	    index++;
        }
    }
//    4. if controls whether the car arrived the destination or not to be enter station again or pass the station.
//         with an additional control of the charge station of which number is 1

        if(rpNextStation == rpDestinationStation) {
      
        if(rpNextStation == 1) {
            directionArray[index] = TURNRIGHT;
        }
        else{
            directionArray[index] = TURNLEFT;
        }
        directionArray[++index] = OVER;
    }
    else {
        directionArray[index] = STRAIGHT;
        directionArray[++index] = STRAIGHT;
        index++;
      
    }
    
//traces along the road until destination station and get next station from optimization module
rpCurrentStation = rpNextStation;
rpNextStation = getNextStation(rpCurrentStation);     //this is the part that we will take from the next station on the road until the destination station
}

  
  //Connect the signal pin to the digital pin 4
  //Adjustable infrared sensor switch code.
  //If there is an obstacle digitalRead returns LOW signal that indicates car arrives the station
  //and rpArrivalFlag turns true else it remains false.
  
}

void isArrived(){
  if(digitalRead(13) == LOW){
     rpArrivalFlag = true;
  }
  else{
    rpArrivalFlag = false;
  }
  
}






